/* -*- c++ -*- */
/* 
 * Copyright 2013 <+YOU OR YOUR COMPANY+>.
 * 
 * This is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 * 
 * This software is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this software; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <stdlib.h>
#include <stdio.h>
#include <gr_io_signature.h>
#include <gr_complex.h>
//#include <blocks/qam_snr_estimator.h>
#include "qam_snr_estimator_impl.h"

#define PI 3.1415926535897931

namespace gr {
  namespace blocks {
    /**************************************************
     * QAM_SNR_ESTIMATOR, Definition Class
     **************************************************/

    qam_snr_estimator::sptr qam_snr_estimator::make(
        unsigned int skip, 
	    const std::vector<int> &snr_seq,
	    const std::vector<gr_complex> &constellation,
	    const std::vector<int> &key_seq,
        int num_errs)
    {
        return gnuradio::get_initial_sptr (new qam_snr_estimator_impl(
          skip, snr_seq, constellation, key_seq, num_errs));
    }

    float qam_snr_estimator::snr() {
        if (!d_sum_power && d_sum_noise == 0) return -1;
        return d_sum_power/d_sum_noise;
        //return (1.*d_training_length)/d_sum_noise[tone]; //assume transmitted power is 1
    }

    /**************************************************
     * QAM_SNR_ESTIMATOR, Implementation Class
     **************************************************/
    qam_snr_estimator_impl::qam_snr_estimator_impl(
        unsigned int skip, 
	    const std::vector<int> &snr_seq,
	    const std::vector<gr_complex> &constellation,
	    const std::vector<int> &key_seq,
        int num_errs
    ) : gr_sync_block("qam_snr_estimator",
            gr_make_io_signature(1, 1, sizeof (gr_complex)),
            gr_make_io_signature2(1, 2, sizeof(float), sizeof(float))),
        d_state(protocol_phases::SKIP),
        d_counter(0),
        d_skip(skip),
        d_skip_counter(0),
        d_training_length(snr_seq.size()),
        d_key_seq(key_seq.size(), 0),
        d_snr_seq(snr_seq),
        d_key_length(key_seq.size()),
        d_key_search(key_seq.size(), 0),
        d_num_errs_allowed(num_errs),
        d_symbol_counter(0),
        d_corr_sum(0),
        d_last_sym(0),
        d_last_expec(0),
        d_last(0),
        d_last_arg(0),
        d_is_differential(true),
        //d_lfsr(0x3901, 0x1fff, 13), //polynomial is x^13 + x^12 + x^11 + x^8 + 1, seed is all ones, 13 bits wide
        //d_controller(NULL),
        d_constellation(constellation)
    {
        d_sum_noise = 0;
        d_sum_power = 0;
        int tmp;

#if 0
        printf("Key: ");
        for(int i; i<d_key_seq.size(); i++){
            printf("%d ",key_seq[i]);
        }
        printf("\n\n");
#endif
#if 1
        printf("SNR seq: ");
        for(int i; i<d_snr_seq.size(); i++){
            printf("%d ",d_snr_seq[i]);
        }
        printf("\n\n");
#endif
#if 2
        d_last = 0;
        printf("SNR seq Diff enc: ");
        for(int i; i<d_snr_seq.size(); i++){
            tmp = (d_snr_seq[i]+d_last)%4;
            printf("%d ",tmp);
            d_last = tmp;
            d_snr_seq[i] = tmp;
        }
        printf("\n\n");
#endif
        //printf ("Last %d\n", d_last);
        d_last = 0;
        int just_to_print = 0;
        if(true){
            //printf("Key UnDiff: ");
            for(int i; i<d_snr_seq.size(); i++){
                if (d_snr_seq[i] < d_last)
                    tmp = d_snr_seq[i] + 4;
                else
                    tmp = d_snr_seq[i];

                just_to_print = (tmp - d_last);
                //printf("%d (in %d last %d) ",just_to_print, key_seq[i], d_last);
                //printf("%d ",just_to_print);
                d_last = d_snr_seq[i];

                //d_snr_seq[i] = just_to_print; TO ACTIVATE UNDIFF SEQUENCE
            }
        }
        //printf("\n\n");

        for(int i; i<d_key_seq.size(); i++){
            d_key_seq[i] = key_seq[i];
        }

        std::reverse(d_key_seq.begin(), d_key_seq.end());
    }

    /*
     * Our virtual destructor.
     */
    qam_snr_estimator_impl::~qam_snr_estimator_impl()
    {
    }

    int
    qam_snr_estimator_impl::work(int noutput_items,
              gr_vector_const_void_star &input_items,
              gr_vector_void_star &output_items)
    {
        const gr_complex *in = (const gr_complex *) input_items[0];
        float *out = (float *) output_items[0];
        float *out_corr;

        int expected_symbol;

        gr_complex to_estimate, to_process;
        unsigned int next = 0;
        unsigned int i, num_errs, demapped, skip, to_skip, diff_dec;
        float corr = 0, in_arg, phase;

#if 0
/*
            printf("\n\nInput ");
            for(int i=0; i<20; i++){
                printf("%f+%f, ", in[i].real(), in[i].imag());
            }
            printf("\n");

            printf("Input byte: ");
            for(int i=0; i<20; i++){
                printf("%d, ", demap(in[i]));
            }
            printf("\n");
*/
            switch(d_state) {
            case protocol_phases::SKIP:
                printf("On SKIP");
                break;
            case protocol_phases::KEY_FILL:
                printf("On KEY_FILL");
                break;
            case protocol_phases::KEY_SEARCH:
                printf("On KEY_SEARCH: corr: %f", d_corr);
                break;
            case protocol_phases::SNR_ESTIMATION:
                printf("On SNR_ESTIMATION");
                printf("\n\nInput ");
                for(int i=0; i<noutput_items; i++){
                    printf("%f+%f, ", in[i].real(), in[i].imag());
                }
                printf("\n");
    
                printf("Input byte: ");
                for(int i=0; i<noutput_items; i++){
                    printf("%d, ", demap(in[i]));
                }
                break;
            case protocol_phases::FORWARDING:
                printf("On FORWARDING:\n");
                break;
            }
            printf("\n\n");
#endif 


        switch(d_state) {
        case protocol_phases::SKIP:
            skip = (noutput_items<(d_skip-d_skip_counter)) ? noutput_items : d_skip-d_skip_counter;
            d_skip_counter += skip;

            if (skip){
                printf("(%d/%d) processing: %d, [noutput: %d]\n", d_skip_counter, d_skip, skip, noutput_items);
            }

            //printf("SNR:to skip %d skiped %d output size %d\n", skip, d_skip_counter, noutput_items);
            if (d_skip_counter >= d_skip){
                d_state = protocol_phases::KEY_FILL;
            } 
            consume_each(skip);
            produce(0,0);
            break;
        case protocol_phases::KEY_FILL:
            to_skip = 0;
            skip = (noutput_items<(to_skip-d_symbol_counter)) ? noutput_items : to_skip - d_symbol_counter;

            if (skip){
                printf("(%d/%d) processing: %d, [noutput: %d]\n", d_symbol_counter, to_skip, skip, noutput_items);
            }

            for(int i = 0; i<skip; i++){
                //printf("adding symbol %d \n", i+d_symbol_counter);
                demapped = demap(in[i]);

                d_key_search.insert(d_key_search.begin(), demapped);
                d_key_search.pop_back();

                d_corr = correlate_key();
                d_corr_sum += d_corr;
            }
            d_symbol_counter += skip;

            //printf("noutput itens %d, key_length %d, d_symbol_counter %d, skip, %d\n\n", noutput_items, d_key_length, d_symbol_counter, skip);
            if (d_symbol_counter >= to_skip){
                d_state = protocol_phases::KEY_SEARCH;
                d_symbol_counter = 0;
                printf("Key Search: Initial Med: %f\n", d_corr_sum/to_skip);
            } 
            consume_each(skip);
            produce(0,0);
            break;
           
        case protocol_phases::KEY_SEARCH:
            i = 0;
            demapped = demap(in[i]);

            // Differential
            if( true ){
                diff_dec = (demapped - d_last_sym) % d_constellation.size();
                d_last_sym = demapped;
            }else{
                diff_dec = demapped;
            }


            d_key_search.insert(d_key_search.begin(), diff_dec);
            d_key_search.pop_back();

            d_symbol_counter ++;

            d_corr = correlate_key();
            d_corr_sum += d_corr;
            d_corr_med = d_corr_sum/(d_symbol_counter+0);

#if 0
            printf("SNR: input %f %f symbol %d nb_syn %d\n", in[0].real(), in[0].imag(), demapped, d_symbol_counter+d_skip);
            printf("SNR: Sink ");
            for (i=0; i<d_key_search.size(); i++){
                printf("%d ", d_key_search[i]);
            }
            printf(" ##### Correlation %f\n", corr);
#endif

            if (abs(d_corr) >= d_num_errs_allowed){
                printf("Found correlation greater than %d in symbol: %d ##### Correlation %f\n", d_symbol_counter, d_num_errs_allowed, d_corr);
                d_state = protocol_phases::SNR_ESTIMATION;
                d_symbol_counter = 0;
                d_first = false;
            }

            if (output_items.size() == 2){
                out_corr = (float*)output_items[1];
                out_corr[0] = abs(d_corr);
                produce(1,1);
            }
            consume(0, 1);
            produce(0, 0);
            break;

        case protocol_phases::SNR_ESTIMATION:
            skip = (noutput_items<(d_training_length-d_counter)) ? noutput_items : d_training_length - d_counter;

#if 0
            //printf ("On init \n");
            if (skip){
                printf("(%d/%d) processing: %d, [noutput: %d]\n", d_counter, d_training_length, skip, noutput_items);
            }
#endif

            for(int i=0; i<skip; i++){
                expected_symbol = d_snr_seq[d_counter];

#if 0
                //printf("i = %d in = %f, %f\n", i, in[i].real(), in[i].imag());
                printf("Received A : %f+%fj, Expected: %f+%f\n", 
                    in[i].real(), in[i].imag(), d_constellation[expected_symbol].real(), d_constellation[expected_symbol].imag());
                printf("Status A: demapped: %d expected %d\n\n",demap(in[i]), expected_symbol);
#endif

                if(false){
                    in_arg = arg(in[i]);
                    if (in_arg<0 | in_arg < d_last_arg)
                        in_arg += 2*PI;
                    phase = (in_arg - d_last_arg) + PI/4;
                    to_estimate = abs(in[i])*gr_complex(cos(phase), sin(phase));

                    //printf("IN: (%f,%f) -> arg(%f), last_arg(%f)\n", in[i].real(), in[i].imag(), in_arg, d_last_arg);
                    //printf("\n\n");

                    d_last_arg = arg(in[i]);
                }else{
                    to_estimate = in[i];
                }

#if 0
                    printf("Status: demapped: %d expected %d\n",demap(in[i]), expected_symbol);
                    printf("Received: %f+%fj, Expected: %f+%f\n\n", 
                        in[i].real(), in[i].imag(), d_constellation[expected_symbol].real(), d_constellation[expected_symbol].imag());
#endif

#if 1
                if (!d_first){

                    d_sum_power += norm(to_estimate);
                    d_sum_expected += norm(d_constellation[expected_symbol]);
                    d_sum_noise += norm(to_estimate - d_constellation[expected_symbol]);

                    out[i] = 10.*log10((float)d_sum_power/(float)d_sum_noise);

                    printf("SNR estimation: %f\n", 10*log10(d_sum_power/d_sum_noise));
                    //printf ("Received_power: %f, Noise_power: %f, Expected_power: %f\n", 
                        //d_sum_power/(d_counter), d_sum_noise/(d_counter), d_sum_expected/(d_counter));
                    log(in[i], d_constellation[expected_symbol], expected_symbol);
                }else{
                    d_first = false;
                }
#endif

                d_counter ++;
            }
            //printf("\nTAG%d/%d\n", d_counter, d_training_length);

            if (d_counter < d_training_length) {
                //printf("counter<training_length: producing %d %d\n", skip, d_training_length);
                consume_each(skip);
                produce(0, skip);
            }else if (d_counter == d_training_length){
                consume_each(skip);
                produce(0, skip);
                d_state = protocol_phases::FORWARDING;
            }else {
                //printf("counter>=training_length: producing nothink\n");
                d_state = protocol_phases::FORWARDING;
                //return -1;
            }
            break;


        case protocol_phases::FORWARDING:
            consume_each(noutput_items);
            produce(0, 0);

            break;
    
        }
        return WORK_CALLED_PRODUCE;
    }

    void qam_snr_estimator_impl::log(gr_complex in, gr_complex expected_symbol, int expected_sym){
        printf("%d/%d (%d)\n", d_counter, d_training_length, d_skip_counter); 
        printf("Status: demapped: %d expected %d\n",demap(in), expected_sym);
        printf("Received: %f+%fj, Expected: %f+%f\n\n", 
            in.real(), in.imag(), expected_symbol.real(), expected_symbol.imag());
        printf("Power Received: %f, Expected: %f, diff %f\n", 
            norm(in), norm(expected_symbol), norm(in)-norm(expected_symbol));
        printf("Actualizing snr: signal power: %f, noise_power: %f SNR %f\n\n\n", 
            d_sum_power, d_sum_noise, 10*log10(d_sum_power/d_sum_noise));
    }

    int qam_snr_estimator_impl::demap(gr_complex sym){
        int near_sym, ret;
        std::vector<float> diff(d_constellation.size(), 0);
        bool log = false;

        // Find near symbol
        if (d_state == protocol_phases::SNR_ESTIMATION){
            log = false;
        }

        if (log)
            printf("On demap: Diff:");
        for(int i=0; i<d_constellation.size(); i++){
            diff[i] = abs(d_constellation[i] - sym);
            if (log)
                printf("%f ",diff[i]);
        }
        if (log)
            printf("\n");
        near_sym = min_element(diff.begin(), diff.end()) - diff.begin();

        return near_sym;
    }

    float qam_snr_estimator_impl::correlate_key(){
        float sum = 0;
        for (int j=0; j<d_key_length; j++) {
            sum += d_key_seq[j] * d_key_search[j];
        }
        return sum;
    }

  } /* namespace blocks */

} /* namespace gr */

